

#include <ipa_building_navigation/ipa_room_planning.h>


int main(int argc, char **argv)
{
    std::cout<< "hello ipa_room_planning_node !"<< std::endl;
	rclcpp::init(argc, argv);
	auto nh = rclcpp::Node::make_shared("ipa_room_planning_node");
    std::shared_ptr<tf2_ros::Buffer> tf_=nullptr;
    IpaRoomPlanning ipa_room_lanning(nh,tf_);
    while(!ipa_room_lanning.prepareData() && rclcpp::ok())
    {
        std::cout << "wait map data 5s again ..."<< std::endl;
    }
    if(ipa_room_lanning.getRoomSegmentation())
    {
        if(ipa_room_lanning.getRoomSequence())
        {
            std::vector<int> optimal_room_sequence;
            for(int i=0; i< ipa_room_lanning.result_seq->checkpoints.size(); i++)
            {
                for(int j=0; j< ipa_room_lanning.result_seq->checkpoints[i].room_indices.size(); j++)
                {
                    optimal_room_sequence.push_back( ipa_room_lanning.result_seq->checkpoints[i].room_indices[j]);
                }
            }

            cv::Mat display;
            for(size_t i = 0; i < ipa_room_lanning.result_seg->room_information_in_pixel.size(); ++i)
            {
                ipa_room_lanning.ipa_planning_param_.start_pos[0]=
                ipa_room_lanning.result_seg->room_information_in_pixel[optimal_room_sequence[i]].room_center.x;
                ipa_room_lanning.ipa_planning_param_.start_pos[1]=
                ipa_room_lanning.result_seg->room_information_in_pixel[optimal_room_sequence[i]].room_center.y;

                cv::Mat map_copy(ipa_room_lanning.exploration_map == optimal_room_sequence[i]+1);
                ipa_room_lanning.getRoomExploration(map_copy,display);
            }

            ipa_room_lanning.Publish();

            if(ipa_room_lanning.ipa_planning_param_.display)
            {
                cv::flip(display, display, 0);
                cv::imshow("exploration client", display);
                cv::waitKey(0);
            }
        }
    }
	//exit
	return 0;
}
